#include <stdlib.h>
#include <string.h>
#include "filter.h"

// indexes in SOS array of filter coefficients
#define B0 0
#define B1 1
#define B2 2
#define A1 4
#define A2 5

// index in W array of internal state values
#define W1 0
#define W2 1

// Initialize a new filter object from its coefficients
void iir_sos_init(iir_sos *f, const double *sos, const double *g, size_t L)
{
	f->sos = sos;
	f->g = g;
	f->w = (double *)malloc(sizeof(*f->w) * 2*L);
	f->L = L;

	iir_sos_reset(f);
}

// Release all resources associated with the filter
void iir_sos_free(iir_sos *f)
{
	free(f->w);
	memset(f, 0, sizeof(*f));
}

// See Proakis, Manolakis. "Digital Signal Processing" 4th Ed, pg 589
static double df2tsos(const double *sos, double *w, double x)
{
	double y = sos[B0]*x + w[W1];
	w[W1] = sos[B1]*x - sos[A1]*y + w[W2];
	w[W2] = sos[B2]*x - sos[A2]*y;

	return y;
}

// Compute output y for input x using Cascade Direct Form-II Transpose implementation
double iir_sos_filter(iir_sos *f, double x)
{
	unsigned int i;
	for(i = 0; i < f->L; i++) {
		x = df2tsos(&f->sos[6*i], &f->w[2*i], f->g[i] * x);
	}
	return x;
}

// Reset the state of the filter by zeroing out w
void iir_sos_reset(iir_sos *f)
{
	memset(f->w, 0, 2*f->L * sizeof(*f->w));
}

/****************************************************************************
							DC Blocking Filter
****************************************************************************/

// see http://www.dspguru.com/dsp/tricks/fixed-point-dc-blocking-filter-with-noise-shaping

#define DC_BLOCK_POLE 0.0001

void dc_block_init(dc_block *d)
{
	d->prev_e = 0;
	d->prev_x = 0;
	d->prev_y = 0;
}

void dc_block_free(dc_block *d)
{
	memset(d, 0, sizeof(*d));
}

double dc_block_filter(dc_block *t, double x)
{
	// d[n] = x[n] - x[n-1]   (no quantization error)
	// y[n] = Quantize{ pole*y[n-1] + d[n] - e[n-1] }
	// e[n] = y[n] - (pole*y[n-1] + d[n] - e[n-1])

	double d = x - t->prev_x;
	double y = DC_BLOCK_POLE * t->prev_y + d - t->prev_e;
	double e = y - (DC_BLOCK_POLE * t->prev_y + d - t->prev_e);

	t->prev_e = e;
	t->prev_x = x;
	t->prev_y = y;

	return y;
}
